import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    # Declare launch arguments
    flight_controller_arg = DeclareLaunchArgument(
        'flight_controller',
        default_value='apm',
        choices=['apm', 'px4'],
        description='Flight controller type (apm or px4)'
    )
    
    config_file_arg = DeclareLaunchArgument(
        'config_file',
        default_value='attitude_control_config.yaml',
        description='Name of the config file'
    )
    
    def launch_setup(context, *args, **kwargs):
        # Get parameter values
        flight_controller = LaunchConfiguration('flight_controller').perform(context)
        config_file = LaunchConfiguration('config_file').perform(context)
        
        # Path to config file
        config_path = PathJoinSubstitution([
            FindPackageShare('usv_run'),
            'config',
            config_file
        ])
        
        # Choose executable based on flight controller
        executable_name = f'attitude_control_node_{flight_controller}'
        node_name = f'attitude_control_{flight_controller}'
        
        # Create node
        attitude_control_node = Node(
            package='usv_run',
            executable=executable_name,
            name=node_name,
            parameters=[config_path],
            output='screen',
            emulate_tty=True,
        )
        
        return [attitude_control_node]
    
    return LaunchDescription([
        flight_controller_arg,
        config_file_arg,
        OpaqueFunction(function=launch_setup)
    ])